#include <dalelib/alife/robothelpers.h>

namespace dalelib
{

//**************************************************************
void createCentipede(
	Robot* _Robot,
	const v3 _Pos,
	const int _NumSegments,
	const float _BodyLength,
	const float _BodyRadius,
	const float _LegLength,
	const float _LegRadius,
	const v3 _SpineAxis,
	const v3 _HipAxis,
	const float _MotorStrength,
	const float _BodyDensity )
{
	const v3 leg_pos =  v3(_BodyRadius*0.5f, 0,0);
	const v3 leg_dir1 = v3(1,-1,0).unit()*_LegLength;
	const v3 body_dir = v3(0,0,1)*_BodyLength;
	const v3 flipx(-1,1,1);

//		v3 pos = v3(0,1+RAND,0)+v3(HFIELD_WIDTH,0,HFIELD_DEPTH)*0.5f;
	v3 pos = _Pos;//+v3(HFIELD_WIDTH,0,HFIELD_DEPTH)*0.5f;
	for(int i=0; i<_NumSegments; ++i)
	{
		const v3 pos0 = pos + leg_pos;
		const v3 pos1 = pos0 + leg_dir1;

		_Robot->addCapsule( _BodyRadius, pos-body_dir*0.5f, pos+body_dir*0.5f, _BodyDensity );
		_Robot->addCapsule( _LegRadius, pos+leg_pos      , pos+(leg_pos+leg_dir1)      , _BodyDensity );
		_Robot->addCapsule( _LegRadius, pos+leg_pos*flipx, pos+(leg_pos+leg_dir1)*flipx, _BodyDensity );

		const int index = i*3;
		_Robot->addHingeJoint( index+0,index+1, _MotorStrength, pos+leg_pos, _HipAxis );
		_Robot->addHingeJoint( index+0,index+2, _MotorStrength, pos+leg_pos*flipx, _HipAxis );
		if(i>0) _Robot->addHingeJoint( index-3, index, _MotorStrength, pos-body_dir*0.5f, _SpineAxis );

		pos += body_dir;
	}
}

//**************************************************************
void createStarfish(
	Robot* _Robot,
	const v3 _Pos,
	const int _NumLimbs,
	const float _BodyRadius,
	const float _LegLength,
	const float _LegRadius,
	const float _MotorStrength,
	const float _BodyDensity )
{
	v3 pos = _Pos;
	_Robot->addCapsule( _BodyRadius, pos, pos+v3(0,0.1f,0), _BodyDensity );

	for(int i=0; i<_NumLimbs; ++i)
	{
		const float ang = TWICEPI*(float(i)+0.5f)/_NumLimbs;
		const v3 pos0 = pos + v3(sinf(ang),0,cosf(ang))*_BodyRadius;
		const v3 pos1 = pos0 + v3(sinf(ang),-0.5,cosf(ang)).unit()*_LegLength;
		const v3 pos2 = pos1 + v3(0.f,-1.f,0.f).unit()*_LegLength;

		_Robot->addCapsule( _LegRadius, pos0, pos1, _BodyDensity );
		_Robot->addCapsule( _LegRadius, pos1, pos2, _BodyDensity );

		v3 axis(cosf(ang),0.f,-sinf(ang));
		const int index = i*2+1;
		_Robot->addHingeJoint(0,    index,  _MotorStrength,pos0,v3(0,1,0));
		_Robot->addHingeJoint(index,index+1,_MotorStrength,pos1,v3(cosf(ang),0.f,-sinf(ang)));
	}
}

//**************************************************************
void createHumanBaby1(
	Robot* _Robot,
	const v3 _Pos,
	const float _MotorStrength,
	const float _BodyDensity )
{
	v3 waist       = _Pos;
	v3 spine       = waist       + v3(  0.0,  2.0,  0.0 );
	v3 chest       = spine       + v3(  0.0,  0.7,  0.0 );
	v3 neck        = chest       + v3(  0.0,  1.0,  0.0 );
	v3 head        = neck        + v3(  0.0,  1.0,  0.0 );
	v3 crown       = head        + v3(  0.0,  0.1,  0.0 );

	v3 lshoulder   = chest       + v3( -1.0,  0.5,  0.0 );
	v3 lelbow      = lshoulder   + v3( -1.0, -1.5,  0.0 );
	v3 lhand       = lelbow      + v3( -1.0, -1.5,  0.0 );

	v3 rshoulder   = chest       + v3(  1.0,  0.5,  0.0 );
	v3 relbow      = rshoulder   + v3(  1.0, -1.5,  0.0 );
	v3 rhand       = relbow      + v3(  1.0, -1.5,  0.0 );

	v3 lhip        = waist       + v3( -1.0,  0.0,  0.0 );
	v3 lknee       = lhip        + v3(  0.0, -2.0,  0.0 );
	v3 lankle      = lknee       + v3(  0.0, -2.0,  0.0 );
	v3 lheel       = lankle      + v3(  0.0,  0.0, -1.0 );
	v3 ltoe        = lankle      + v3(  0.0,  0.0,  0.1 );

	v3 rhip        = waist       + v3(  1.0,  0.0,  0.0 );
	v3 rknee       = rhip        + v3(  0.0, -2.0,  0.0 );
	v3 rankle      = rknee       + v3(  0.0, -2.0,  0.0 );
	v3 rheel       = rankle      + v3(  0.0,  0.0, -1.0 );
	v3 rtoe        = rankle      + v3(  0.0,  0.0,  0.1 );



	_Robot->addCapsule( 0.8f, waist,      spine,    _BodyDensity ); // 0
	_Robot->addCapsule( 1.0f, spine,      chest,    _BodyDensity ); // 1

	_Robot->addCapsule( 0.4f, lshoulder,  lelbow,   _BodyDensity ); // 2
	_Robot->addCapsule( 0.4f, lelbow,     lhand,    _BodyDensity ); // 3

	_Robot->addCapsule( 0.4f, rshoulder,  relbow,   _BodyDensity ); // 4
	_Robot->addCapsule( 0.4f, relbow,     rhand,    _BodyDensity ); // 5

	_Robot->addCapsule( 0.4f, lhip,       lknee,    _BodyDensity ); // 6
	_Robot->addCapsule( 0.4f, lknee,      lankle,   _BodyDensity ); // 7
	_Robot->addCapsule( 0.4f, lheel,      ltoe,     _BodyDensity ); // 8

	_Robot->addCapsule( 0.4f, rhip,       rknee,    _BodyDensity ); // 9
	_Robot->addCapsule( 0.4f, rknee,      rankle,   _BodyDensity ); // 10
	_Robot->addCapsule( 0.4f, rheel,      rtoe,     _BodyDensity ); // 11

	_Robot->addCapsule( 0.5f, chest,      head,     _BodyDensity ); // 12
	_Robot->addCapsule( 1.0f, head,       crown,    _BodyDensity ); // 13




	_Robot->addHingeJoint(  0,  1, _MotorStrength, spine,       v3(1,0,0) );

	_Robot->addHingeJoint(  1,  2, _MotorStrength, lshoulder,   v3(0,0,1) );
	_Robot->addHingeJoint(  2,  3, _MotorStrength, lelbow,      v3(0,0,1) );

	_Robot->addHingeJoint(  1,  4, _MotorStrength, rshoulder,   v3(0,0,1) );
	_Robot->addHingeJoint(  4,  5, _MotorStrength, relbow,      v3(0,0,1) );

	_Robot->addHingeJoint(  0,  6, _MotorStrength, lhip,        v3(1,0,0) );
	_Robot->addHingeJoint(  6,  7, _MotorStrength, lknee,       v3(1,0,0) );
	_Robot->addHingeJoint(  7,  8, _MotorStrength, lankle,      v3(1,0,0) );

	_Robot->addHingeJoint(  0,  9, _MotorStrength, rhip,        v3(1,0,0) );
	_Robot->addHingeJoint(  9, 10, _MotorStrength, rknee,       v3(1,0,0) );
	_Robot->addHingeJoint( 10, 11, _MotorStrength, rankle,      v3(1,0,0) );

	_Robot->addHingeJoint(  1, 12, _MotorStrength, chest,       v3(1,0,0) );
	_Robot->addHingeJoint( 12, 13, _MotorStrength, head,        v3(0,1,0) );

}

} // namespace dalelib
